package javavis.jip3d.functions;

import java.util.ArrayList;

import javavis.base.Function3DGroup;
import javavis.base.JIPException;
import javavis.base.ParamType;
import javavis.jip3d.geom.MyTransform;
import javavis.jip3d.geom.MyTransform3D;
import javavis.jip3d.gui.Function3D;
import javavis.jip3d.gui.FunctionParam;
import javavis.jip3d.gui.ScreenData;
import javavis.jip3d.gui.ScreenOptions;
import javavis.jip3d.gui.dataobjects.PointSet3D;
import javavis.jip3d.gui.dataobjects.Trajectory;
import javavis.jip3d.gui.dataobjects.Trajectory3D;

public class FICPTrayectory extends Function3D {

	public FICPTrayectory() {
		super();

		this.allowed_input = ScreenOptions.tTRAJ2D | ScreenOptions.tTRAJ3D;
		this.group = Function3DGroup.Egomotion;
		
		FunctionParam p1 = new FunctionParam("Ground Truth", ParamType.BOOL);
		p1.setValue(false);
		
		FunctionParam p2 = new FunctionParam("Step", ParamType.INT);
		p2.setValue(1);

		addParam(p1);
		addParam(p2);
	}

	@Override
	public void proccessData(ScreenData scr_data) throws JIPException {
		result_list = new ArrayList<ScreenData>();
		PointSet3D points1, points2;
		int file_number;
		int cont;
		String file_name;
		Trajectory traj;
		String path;
		int step = paramValueInt("Step");
		MyTransform transform, new_transform, total_transform;
		double errX, errY, errZ, errAX, errAY, errAZ;
		double errorPos, errorAng, errorPosTotal=0, errorAngTotal=0;
		int numPoses = 0;
		
		boolean groundTruth = paramValueBool("Ground Truth");
		
//		FICP Icp = new FICP();
		FFastICP Icp = new FFastICP();
		if(groundTruth) Icp.setParamValue("Verbose", false);
		else Icp.setParamValue("Verbose", true);
		Trajectory3D new_traj = new Trajectory3D(new ScreenOptions());
		
		traj = (Trajectory)scr_data;
		file_number = traj.files.size();
		path = traj.path;

		double prog_inc = 100.0/(file_number-1);
		file_name = traj.files.get(0);
		new_traj.files.add(file_name);
		new_traj.transforms.add(traj.transforms.get(0));

		points2 = new PointSet3D(new ScreenOptions());
		points2.readData(file_name, path);
		new_transform = new MyTransform3D();
		
		total_transform = new MyTransform3D();
		for(cont=1;cont<file_number;cont++)
		{

			points1 = points2;
			file_name = traj.files.get(cont);
			
			transform = new MyTransform3D(traj.transforms.get(cont));
			total_transform.applyTransform(transform);

			if(cont%step==0)
			{
				numPoses++;
				points2 = new PointSet3D(new ScreenOptions());
				points2.readData(file_name, path);
				Icp.setParamValue("Next Object", points2);
				Icp.proccessData(points1);
//				result_list.add(points2);
				new_transform = Icp.getTransform();
				if(groundTruth)
				{
					errX = total_transform.trX - new_transform.trX;
					errY = total_transform.trY - new_transform.trY;
					errZ = total_transform.trZ - new_transform.trZ;
					errAX = total_transform.angX - new_transform.angX;
					errAY = total_transform.angY - new_transform.angY;
					errAZ = total_transform.angZ - new_transform.angZ;
					errorPos = errX*errX + errY*errY + errZ*errZ;
					errorPosTotal += errorPos;
					errorPos = Math.sqrt(errorPos);
					errorAng = errAX*errAX+errAY*errAY+errAZ*errAZ;
					errorAngTotal += errorAng;
					errorAng = Math.sqrt(errorAng);
					System.out.println(cont+": Pos: "+errorPos+" Ang: "+errorAng);
				}
				else
					System.out.println(file_name+" "+new_transform);
				transform.applyTransform(new_transform);
				new_traj.files.add(file_name);
				new_traj.transforms.add(transform);
				total_transform = new MyTransform3D();
			}
			progress += prog_inc;

		}
		new_traj.scr_opt.num_points = file_number;
		new_traj.path = traj.path;
		new_traj.name = "fastICP.6dof";
		result_list.add(new_traj);
		
		if(groundTruth)
		{
			System.out.println("Pose RMS: "+Math.sqrt(errorPosTotal/numPoses));
			System.out.println("Ang. RMS: "+Math.sqrt(errorAngTotal/numPoses));
		}

	}

}
